#include "framework.h"

FrameWork::FrameWork() : is_init_(false)
{
    sigma_init[0] = 5.0;
    sigma_init[1] = 5.0;
    sigma_init[2] = 0.01;

    sigma_lane_fixed[0] = 5.0;
    sigma_lane_fixed[1] = 0.5;
    sigma_lane_fixed[2] = 0.01;

    sigma_sign_pole_fixed[0] = 0.5;
    sigma_sign_pole_fixed[1] = 0.5;
    sigma_sign_pole_fixed[2] = 0.01;

    sigma_no_fix[0] = 5.0;
    sigma_no_fix[1] = 5.0;
    sigma_no_fix[2] = 0.01;

    sigma_predict[0] = 0.3;
    sigma_predict[1] = 0.3;
    sigma_predict[2] = 0.01;
}

FrameWork::~FrameWork()
{

}

bool FrameWork::initialized() const
{
    return is_init_;
}

void FrameWork::Init(const GeoPoint& gps_point, const hdmap_frame& hdmap_f, const Camera& camera, DbgFrame* dbg_frame)
{
    /*if (!pf_.initialized())
    {
        pf_.init(gps_point.x, gps_point.y, gps_point.z, gps_point.bearing, sigma_init, 3000);
    }
    else
    {
        double sigma_predict[3] = {0.3, 0.3, 0.01};
        double delta_t = (gps_point.timestamp - prev_point.timestamp) / 1000.0;
        pf_.prediction(delta_t, sigma_predict, prev_point.speed, prev_point.yawrate);
        pf_.GetCurrentLoc(gps_point.x, gps_point.y, gps_point.z, gps_point.bearing);
    }
    LFM::LaneMatching lfm;
    if (lfm.Calculate_Pose(dlFrame, hdmap_f.hdmap_lanes, gps_point, camera, pose, rmse, dbg_frame.get()))
    {
    }*/
}
